#include <string.h>

#include "parani.h"
#include "uart.h"

#define MAX_RESPONSE 100


static void parani_send_cmd(char * cmd)
{
	printf(cmd); printf(CR);
}

int parani_get_response(char * resp, int max_resp)
{
	FILE * stream = &bluetooth_uart_stdin;
	int i;
	char * addr = resp;
	int non_ws_found = 0;
	do {
		if ((i = getc (stream)) < 0)
			break;
		if (i == '\r')
			continue;
		if (i == '\n') {
			if (non_ws_found)
				break;
			else
				continue;
		}
		non_ws_found = 1;
		if (addr)
			*(char *)addr++ = i;
	} while (1);
	if (addr)
		*(char *)addr = 0;
	return 0;
}

static int parani_expect_response(char * expected_response)
{
	int err = 0;
	char resp[MAX_RESPONSE];

	err = parani_get_response(resp, MAX_RESPONSE);
	if (err != 0)
		return err;

	// check if the response is what we expected
	if (strcmp(resp, expected_response) != 0)
		return -12;
	return 0;
}

/*
 * Send the `cmd` command to PARANI, append a Carriage Return.
 * Read the first response from PARANI. 
 * If if differs from the `expected_response` return -1.  (error)
 * If it is the same as the `expected_response` return 0. (success)
 */
static int parani_send_cmd_expect_response(char * cmd, char * expected_response)
{
	parani_send_cmd(cmd);
	return parani_expect_response(expected_response);
}

/*
 * reset PARANI to factory defaults
 */
int parani_reset_factory_defaults()
{
	return parani_send_cmd_expect_response("AT&F", "OK");
}

/*
 * Check connection status to parani device
 */
int parani_check_physical_device_connection()
{
	return parani_send_cmd_expect_response("AT", "OK");
}

/*
 * Wait for inquiry and connection from other Bluetooth devices.
 * This function does not return until a connection is established 
 * or an error occurs.
 */
int parani_wait_for_connections_forever(char id[13])
{
	char resp[8 + 12 + 10]; // 8 = "CONNECT ". 12 = 001641B5CDF8 . 1 = '\0'
	int err = parani_send_cmd_expect_response("AT+BTSCAN", "OK");
	if (err != 0)
		return err;

	err = parani_get_response(resp, sizeof(resp));
	if (err != 0)
		return err;

	// check if it starts with "CONNECT "
	if (strncmp(resp, "CONNECT ", strlen("CONNECT ")) != 0)
		return -13;

	strncpy(id, resp + strlen("CONNECT "), 12);
	return 0;
}


int parani_set_name(char * name)
{
	char cmd[100];
	snprintf(cmd, 99, "AT+BTNAME=\"%s\"", name);
	return parani_send_cmd_expect_response(cmd, "OK");
}

int parani_get_register(int reg, char * str, int size)
{
	int err = 0;
	char cmd[100];
	snprintf(cmd, 99, "ATS%d?", reg);
	parani_send_cmd(cmd);
	
	err = parani_get_response(str, size);
	if (err != 0)
		return err;
	return parani_expect_response("OK");
}

int parani_get_all_registers(char * str, int size)
{
	int err = 0;
	parani_send_cmd("AT&V");
	err = parani_get_response(str, size);
	if (err != 0)
		return err;
	return parani_expect_response("OK");
}

int parani_get_register_str(int reg, char * str, int size)
{
	int err = 0;
	char cmd[100];
	snprintf(cmd, 99, "ATS%d?", reg);
	parani_send_cmd(cmd);
	err = parani_get_response(str, size);
	if (err != 0)
		return err;
	return parani_expect_response("OK");
}

int parani_set_register(int reg, char * val)
{
	char cmd[100];
	snprintf(cmd, 99, "ATS%d=%s", reg, val);
	return parani_send_cmd_expect_response(cmd, "OK");
}

int parani_set_class_of_device(char * cod)
{
	return parani_set_register(43, cod);
}

int parani_set_class_of_devicetoy_robot()
{
	// robot + toy + generic IAC
	return parani_set_class_of_device("201000");
}

int parani_set_class_of_device_from_manual()
{
	return parani_set_class_of_device("020300");
}
int parani_init()
{
	int err = 0;
	err = parani_set_name("AWESOME-O");
	if (err != 0)
		return err;
	return 0;
}
